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Devices for lifting, translating and precisely placing payloads are critical for efficient 
Earth-based construction operations. Both recent and past studies have demonstrated that 
devices with similar functionality will be needed to support lunar outpost operations. 
Although several designs have been developed for Earth based applications, these devices 
lack unique design characteristics necessary for transport to and use on the harsh lunar 
surface. These design characteristics include: a) lightweight components, b) compact 

packaging for launch, c) automated deployment, d) simple in-field reconfiguration and 
repair, and e) support for tele-operated or automated operations. Also, because the cost to 
transport mass to the lunar surface is very high, the number of devices that can be dedicated 
to surface operations will be limited. Thus, in contrast to Earth-based construction, where 
many single-purpose devices dominate a construction site, a lunar outpost will require a 
limited number of versatile devices that provide operational benefit from initial construction 
through sustained operations. 

The first generation test-bed of a new high performance device, the Lunar Surface 
Manipulation System (LSMS) has been designed, built and field tested. The LSMS has 
many unique features resulting in a mass efficient solution to payload handling on the lunar 
surface. Typically, the LSMS device mass is estimated at approximately 3% of the mass of 
the heaviest payload lifted at the tip, or 1.8 % of the mass of the heaviest mass lifted at the 
elbow or mid-span of the boom for a high performance variant incorporating advanced 
structural components. 

Initial operational capabilities of the LSMS were successfully demonstrated during field 
tests at Moses Lake, Washington using a tele-operated approach. Joint angle sensors have 
been developed for the LSMS to improve operator situational awareness. These same 
sensors provide the necessary information to support fully automated operations, greatly 
expanding the operational versatility' of the LSMS. This paper develops the equations 
describing the forward and inverse relation between LSMS joint angles and Cartesian 
coordinates of the LSMS tip. These equations allow' a variety of schemes to be used to 
maneuver the LSMS to optimize the maneuver. One such scheme will be described in detail 
that eliminates undesirable swinging of the payload at the conclusion of a maneuver, even 
when the payload is suspended from a passive rigid link. The swinging is undesirable when 
performing precision maneuvers, such as aligning an object for mating or positioning a 
camera. Use of the equations described here enables automated control of the LSMS greatly 
improving its operational versatility. 
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Nomenclature 


R 

= Reach of LSMS 

1R 

= Height to shoulder, 0 < 1 ] < 1 

PR 

= Distance from shoulder to elbow, 0 < /? < 1 

LSMS 

= Lunar Surface Manipulation System 

o, 

= Waist joint angle 

e 2 

= Shoulder joint angle 

e. 

= Elbow joint angle relative to arm 

o. 

= Lifting link angle from the vertical 

y puch 

= Wrist pitch angle 

y roll 

= Wrist roll angle 

y y aw 

= Wrist yaw angle 


I. Introduction 

D evices for lifting, translating and precisely placing payloads are critical for efficient Earth-based construction 
operations. Both recent and past studies have demonstrated that devices with similar functionality will be 
needed to support lunar outpost operations. 1-2 Lunar payloads include: a) prepackaged hardware and supplies 
that must be unloaded from landers and then accurately located at their operational site, b) sensor packages used for 
periodic inspection of landers, habitat surfaces, etc., and c) local materials such as regolith that require excavation 
and grading. 3 Although several designs have been developed for Earth based applications, these devices lack unique 
design characteristics necessary for transport to and use on the harsh lunar surface. 4 These design characteristics 
include: a) lightweight components, b) compact packaging for launch, c) automated deployment, d) simple in-field 
reconfiguration and repair, and e) support for tele-operated or automated operations. Also, because the cost to 
transport mass to the lunar surface is very high, the number of devices that can be dedicated to surface operations 
will be limited. Thus, in contrast to Earth-based construction, where many single-purpose devices dominate a 
construction site, a lunar outpost will require a limited number of versatile devices that provide operational benefit 
from initial construction through sustained operations. 

The first generation test- 
bed of a new high performance 
device, the Lunar Surface 
Manipulation System (LSMS) 
has been designed and built. 5 
The LSMS has many unique 
features resulting in a mass 
efficient solution to payload 
handling on the lunar surface. 
Typically, the LSMS device 
mass is estimated at 
approximately 3% of the mass 
of the heaviest payload lifted at 
the tip. or 1.7 % of the mass of 
the heaviest mass lifted at the 
elbow or mid-span of the boom 
for a high performance variant 
incorporating advanced 

structural components. 6 

Figure 1. Payload handling operations performed at Moses Lake field During laboratory tests the 
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LSMS closely matched structural analysis predictions. Initial operational capabilities of the LSMS were 
successfully demonstrated during field tests at Moses Lake. Washington (Fig. 1) using a tele-operated approach. 5 

A natural enhancement to the current tele-operated (person in the loop) control is automated control. Automated 
control is necessary for several reasons including the ability to direct the LSMS from remote sites (for example the 
lunar outpost or earth) and to enable interaction with global planning tools to evaluate different concepts of 
operations. The joint angle sensors necessary to provide an operator with situational awareness during tele-operated 
maneuvers are directly applicable to automated control, thus no burden is placed on the system design to support 
automated control. 

This paper will describe the design and installation of joint angle sensors to support automated operations and 
situational awareness for a remote operator. Then the equations for the forward and inverse kinematics necessary 
for automated control will be provided. The forward kinematic equations describe the location of the tip of the 
LSMS when the joint angles are known, the inverse kinematic equations are used to determine the joint angles 
necessary to position the tip at a desire location. The inverse kinematics allows direct translation from Cartesian 
goals to joint level commands to achieve those goals. For example, the inverse kinematics enable straight line 
Cartesian moves of the LSMS in either tip or potentially more useful camera defined frames providing intuitive 
motion control to the operator. A common scheme for describing robotic manipulators uses four parameters called 
the Denavit-Hartenburg parameters. The Denavit-Hartenburg parameters for the LSMS are provided in appendix A. 

The forward and inverse kinematic equations allow a variety of schemes to be used to maneuver the LSMS to 
optimize the maneuver. One such scheme will be described in detail that eliminates undesirable swinging of the 
payload at the conclusion of a maneuver, even when the payload is suspended from a passive rigid link called a 
lifting link. This results in the lifting link and payload behaving like a double pendulum. It is possible to eliminate 
undesirable swinging motion at the conclusion of a maneuver using optimal control techniques. Three approaches 
have been investigated and are discussed in this paper, and numerical results obtained with each scheme are 
provided. 


II. LSMS Test-Bed Overview 

Figure 2 identifies the major components of the LSMS Test-Bed, and Table 1 provides the corresponding 
dimensions for the first generation test-bed constructed in 2008. Referring to the figure, the primary design drivers 
for the LSMS are the maximum reach. R , the shoulder height, ijR where 0 < // < 1 . and the maximum payload 
mass to be lifted at the wrist or elbow in lunar or Mars gravity. The LSMS design scales easily over a wide range of 
reaches and payload masses. Because of the efficient structural design, the total device mass for a device based on 
composite trusses is typically approximately 3% of the maximum load lifted at the wrist or 1 .7% of the maximum 
load lifted at the elbow. Details on the scaling laws are provided in Refs. 6 and 7. The LSMS has 3 primary degrees 
of freedom, provided by revolute joints identified by the red dotted call-outs in the figure. Using the inertial frame 
identified in Fig. 2 and starting at the ground interface (or base) of the device, the first revolute joint is referred to as 
the waist joint and it provides a simple rotation about the positive z axis represented by the angle 9 X measured from 
the positive x axis. The second revolute joint is located at the top of the king post at a distance fjR (0 < 77 < 1 ) 
along the positive z axis. This joint is called the shoulder joint, and it provides a simple rotation about an axis 
parallel to positive y axis that is represented by the angle measured from a horizontal line passing through the 
shoulder. The third revolute joint, called the elbow joint, is positioned at a distance fiR (0 < /? < 1 ) in the 
positive x direction from the shoulder. It enables a simple rotation about an axis parallel to the positive y axis that is 
represented by the angle 6 i measured from a line passing through the shoulder and elbow joints. The joints are 

actuated by electric motors. The electric motors drive hoists to actuate the shoulder and elbow while an electric 
motor drives the waist rotation directly. Finally, the wrist is located at the end of the foreann by continuing along 
positive x a distance R{ 1 — /?) . In the figure a lifting link, or rigid connection used to lift payloads is shown. The 

angle of the lifting link relative to the vertical is identified by 0 4 . 
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Figure 2. LSMS Naming Convention. 

Table 1, LSMS Test-Bed Dimensions and Capability 



Height 
(rjR [m]) 

Wrist Reach 
(R[m]) 

Elbow Reach 
( pR |m]) 

Lunar Wrist 
Capacity([kg]) 

Lunar Elbow 
Capacity([kg]) 

Generation I 

3.75 

7.5 

3.76 

1000 

1765 


III. Joint Angle Sensors for the LSMS 

Initial development of the LSMS focused on mechanical design of the test-bed with an eye toward automated 
operations in the future. Having successfully demonstrated the LSMS in the field in June of 08, the focus of 09 was 
to develop a sensing system for the joint angles of the LSMS as well as to develop the forward and inverse 
kinematics for the LSMS in preparation for automated control. Joint sensors were integrated to measure each active 
joint; waist, shoulder and elbow shown in Fig. 2. In each case a cam is affixed to one side of the joint and the sensor 
package is affixed to the other. The sensor package is based on a pair of Keyence GT series displacement sensors 
that measure the radial distance from the joint axis. This distance varies linearly with angle and is controlled by the 
profile on the cam. Two profiles were machined into each cam. This provides a redundant measurement of position 
and in addition, the difference between the 2 position measurements also varies linearly with angle providing a 
robust method to counteract eccentricity in the cam mounting. Figures 3 and 4 depict the details of the sensor 
mounting at the elbow. Figure 3 is a view from under the LSMS looking up in the positive z direction. A close up 
of the sensors and cam is shown in Fig. 4. Cabling to the sensors has been omitted from the figures for clarity. The 
sensors are mounted to the forearm via a bracket, the cam is affixed to the arm and rotates about the elbow joint pin. 
The bracket houses 2 plungers that transmit the radial displacement to the sensor heads while absorbing any 
transverse loads on the sensors. In addition, use of the plungers reduces the width of the cam. The mounting at the 
shoulder is very similar to that at the elbow and uses an identical cam. Figures 5 and 6 depict the mounting at the 
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waist. The waist uses a larger cam. however the change in radial displacement is the same as the elbow and 
shoulder enabling the same sensor model to be used at all locations. 



Figure 3. Bottom View of Elbow Sensors. 


Figure 4. Elbow and Shoulder Sensor System. 



Figure 5. Side View of Waist Sensors. 



Figure 6. Waist Cam and Sensor System. 


IV. Forward Kinematics of the LSMS 

This section will develop the equations that describe the position and orientation of each link with respect to the 
inertial frame when the joint angles are known. Of particular importance is a set of equations that describe the 
location of the wrist in the inertial frame when the joint angles are known. The previous section described one 
method for measurement of the joint angles of the LSMS. Three transformations can be used to relate the relative 
position and orientations of the links of the LSMS, and a fourth transformation describes the relationship between 
the wrist and the elbow frame. The transformations will be defined following the notation provided by Craig using 
the local reference frames identified in Fig. 7. 8 

In Craig’s notation, a 4x4 homogeneous transform is used to describe the relative position and orientation of two 
frames. Equation (1) depicts a homogenous transform describing the position and orientation of frame B in frame A 
as indicated by the leading sub and super scripts of T. The upper left 3x3 of the 4x4 homogenous transform is a 
rotation matrix, the columns of which are unit vectors defining the directions of the axes of B in frame A, i.e. if 

[tfj,t7 2 ,tf 3 ] are three unit vectors fixed in frame A and j^,,&,,6 3 J are three unit vectors fixed in B, then 
r st = a S »b t ,(s,t = 1,2, 3) . The vector ^ p x ,p x ,P- in Eq. (1) locates the origin of frame B in frame A. 
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Figure 7. Local Reference Frames. 
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Referring to Fig. 7, local reference frames are affixed to each link. i.e. each group of components that move as a 
unit are shown with a similar color in the figure and this color is also used for the axes of the reference frame. The 
local frames shown in the figure do not follow the Denavit-Hartenberg convention due to consistency with other 
analysis efforts. For completeness, a set of Denavit-Hartenberg parameters along with corresponding reference 
frames has been included in appendix A. Starting at the base, or inertial frame, the first local frame is the waist 
frame upon which the blue components move, followed by the shoulder frame about which the magenta components 
rotate, then the elbow frame in which the green components rotate as a unit, and finally the wrist frame. The lifting 
link shown in black rotates relative to the wrist about a pin parallel to the y axis of the wrist frame. The lifting link 
is a passive link. Using the reference frames, it is straightforward to describe the location of the wrist with respect to 
the inertial frame. Starting at the base of the LSMS and working toward the wrist, the relation between the local 
reference frames are given by: 


inertial y 

waist * 


COS 


w 

sin ( #, ) 
0 
0 


-sinj #, ) 
cos($, ) 
0 
0 


0 0 
0 0 
1 0 
0 1 


( 2 ) 
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shoulder 


T = 


shoulder 

elbow 


T = 



cos (9 ,) 

0 

sin (9 ,) 

0 


0 

1 

0 

0 


-sin (9,) 

0 

cos{9 2 ) 

f] R 


0 

0 

0 

1 


cos(9 3 ) 

0 

sin(9 3 ) 

PR 


0 

1 

0 

0 


-sin(9 3 ) 

0 

cos{9 3 ) 

0 


0 

0 

0 

1 

fl 0 0 

*0- 

-*)1 



(3) 


(4) 


elbow 

wrist 


T = 


0 1 
0 0 
0 0 


(5) 


Then the forward kinematics of the LSMS, i.e. the definition of the wrist in the inertial frame, is given by 


inertial 


rp _ inertial rp waist rp shoulder rp elbow rp 

wrist waisn shoulder ■* elbow^ wrisn 


( 6 ) 


T = 


Cos(9 x )Cos(0 2 + 9 3 ) -Sin(9 x ) Cos{9 x )Sin(0 2 + 9 3 ) RCos(9 x ) [ pCos{9 2 ) + (1 - p)Cos{9 2 + 9 3 )] 
Cos (6, + 6 * 3 )Sin(9 x ) Cos(9 x ) Sin(6 x )Sin{6 2 + 6 3 ) RSin(9 x ) [/ 3Cos(0 2 ) + (1 - p)Cos{9 2 + 9 3 )] 
-Sin(0 2 + 9 } ) 0 Cos{6 2 +0 3 ) R[jj- pSin(0 2 ) - (1 - P)Sin(0 2 +0 3 )] 

0 0 0 1 


Equation ( 6 ) is commonly used to transform a target location in the wrist frame into a target location in the inertial 
frame. This transformation occurs during operator or sensor driven motion control where the camera or sensor 
system is attached to the wrist. The inverse kinematics developed in the next section are then used to convert the 
target location into corresponding joint angles which can be used to drive the motors of the robot so that the wrist 
arrives at the desired target location. It is straight forward to locate any other frame of Fig. 7 relative to the inertial 
frame by a similar process, thus with knowledge of the joint angles, the location of all the links are precisely known. 

V. Inverse Kinematics of the LSMS 

The inverse kinematic problem is generally much more difficult than the forward kinematic problem for a serial 
manipulator, however because of the design simplicity of the LSMS, the inverse solution is not complex. The 

inverse kinematic problem is to compute the set of joint angles, the joint vector , that locates the wrist 

at a target position. Note the LSMS has 3 degrees of freedom, thus only three of six coordinates describing the 
position and orientation of the wrist may be specified. In the following, two sets of target coordinates will be 

specified. The first is the orientation of the wrist with respect to inertial frame, i.e. ^/ wll , / vaw , 7 pitC h ] • This set 

of relations can be determined by inspection and they are: 

Y pitch — @2 ^3 

Yya» = 9 l 

YroII ( no control authority) 

Note, if a particular operation requires wrist roll, then it can easily be accommodated by an appropriately designed 
end-effector or tool attached at the wrist. 
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The second set of three target coordinates will be taken to be the position of the wrist in Cartesian space relative 
to the inertial frame, i.e. ^/ v ,f v ,f J . It is possible to solve for the explicit position of the tip in terms of the joint 
angles by selecting the wrist position vector from Eq. (6) and setting it equal to the desired target position, i.e. 


Rcos{6 x ) [ pcos(0 2 ) + (1 - p)cos(0 2 + 0 3 )] 

( 7 ) 

Rsin{6 x ) [ pcos^O , ) + (1 - p)cos{6 , + 0 3 )] 

( 8 ) 

R [rj - psin{0 2 )- (1 - P)sin{0 2 + < 9 3 )] 

( 9 ) 


While the equations appear simple, the solution is lengthy. These equations were solved using Mathematica and 
the corresponding script commands are given in appendix B. Only the solution for 0 X is tractable and given by Eq. 
( 1 0) below. Equation ( 1 0) agrees with intuition based on a top view looking down the z axis of the inertial frame. 

6 X = ATAN2(t y ,t x ) ( 10 ) 


Multiple solutions for a specified 
target position exist corresponding 
to location of the elbow joint above 
or below a line from the wrist to 
shoulder as depicted in Fig. 8. 

These two configurations are called 
elbow up and elbow down 
respectively. In the figure the elbow 
up configuration is shown ghosted. 

When the LSMS reaches full 
extension, i.e. 0 3 = 0, it is necessary 

to select which configuration to 
pursue for the current maneuver. 

This is managed by the control 
software. 

VI. Ending a Maneuver with 
Payload at Rest 

The mechanical system of 
interest, sans the payload, is 
described in Sec. II. As discussed 
there, a lifting link is attached to the 
forearm at the wrist by means of a 

revolute joint. The lower end of the Fi 8 . Tw0 Solutions for a Wrist Location: Elbow Up vs. Down 
lifting link is used m grappling a 

payload, and this connection is also modeled as a revolute joint. The two revo lute joints connected to the lifting link 
have axes that are parallel to each other. Simple rotation of the payload relative to the lifting link is described by an 
angle # 5 . The lifting link and payload thus have the configuration of a double pendulum, which is suspended from 
the forearm. 

No motors are placed at the revolute joints connected to either end of the lifting link: hence, motion of the king 
post, arm, and forearm can result in oscillations of the lifting link and payload. Such swinging motion of the 
payload is undesirable for two reasons. First, it makes precise placement of the payload difficult: the payload may 
collide with other objects in close proximity to the intended final position. Depending on the amplitude of the 
oscillations and the amount of damping in the two revolute joints, the tune required for the swinging to be damped 
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out may represent a significant and unwanted delay in operations. Second, structural vibrations in the LSMS could 
be induced by swinging motion of the payload. 

These considerations serve as motivation to find a way of maneuvering the LSMS and payload so that swinging 
of the lifting link and payload are eliminated at the conclusion of the maneuver. The problem of bringing the 
payload to rest when the maneuver ends is amenable to solution through the use of techniques of optimal control. 

A. Equations of Motion 

In order to solve an optimal control problem, one must have available a set of first-order differential equations 
governing the “state” of the system to be controlled. In other words, the kinematical differential equations and the 
dynamical equations of motion of the system are required. For this task, we employ the method of Kane as 
discussed in Refs. 9 and 10. The method involves choosing motion variables, also known as generalized speeds. In 
the case of a serial manipulator consisting of bodies fastened together by n re volute joints, the time derivatives of the 

joint angles 0 r (r = 1 are natural choices for motion variables, denoted here by CO r . The following 
relationships serve as definitions of the motion variables and as kinematical differential equations. 


0 r = C 0 r (r = l, 

(Error! Bookmark not defined. 1 1) 

It is expedient to regard m of the revolute joints (m < n) as driven by motors that exert couples whose torques have 
directions parallel to the joint axes, and magnitudes that are the absolute values of the scalars U r (r = 1,..., m). Each 
body belonging to the manipulator is subject to gravitational force, considered to be applied at the body's mass 
center. The software AUTOLEV discussed in Ref. 10 makes use of computer algebra to derive explicit analytical 
dynamical equations of motion, given here in functional form as 


°>r =S r {.0 xi ...,0 n ,CO l ,...,CO n ,U l , ... ,U m ,t ) ( /* = 1 , 


(12) 


where t denotes time. 

The system under consideration consists of five bodies (king post, arm. forearm, lifting link, and payload) and 
five revolute joints; thus, n = 5. One motor is used to drive each of three joints (waist joint, shoulder joint, and 
elbow joint); therefore, m = 3. For the purposes of this study, all bodies are considered to be rigid; appendix C 
contains the mass, inertia scalars, and mass center position for each of the five bodies, as well as position 
information for each of the five joints. 

B. Review of Optimal Control Problem Formulation 

The problem of bringing the manipulator payload to rest at the conclusion of a maneuver can be formulated as a 
problem in optimal control. As discussed (for example) in Ref. 11. the objective is to determine the functions 
Mj(f), . . ., U m (t) that minimize (or maximize) a performance index J expressed as 


J = <fi[x(t f )\+ j L(x,u,t)dt 

f 0 


( 13 ) 


subject to nonlinear differential equations of state 

x = f(x,u,t ) (14) 

and subject to terminal constraints 
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'*'[*('/)]= 0 


(15) 


In the case of a serial manipulator, 0 X , . . . , 6 n , (Q x , . . . , 0 ) n constitute the elements of the state x. The 
differential equations (14) are simply a collection of Eqs. (11) and (12). In this paper we concern ourselves with 
problems for which initial values of all elements of the state, are specified. Functions |x(/^ )j = 0 (r = 

1, ..., q ) of the state at the conclusion of the maneuver, can be specified, where q < 2n for L ^ 0. Such 

requirements are often referred to as terminal constraints, and are represented by Eq. (15). A requirement that the 
manipulator and payload be in a certain configuration at the end of the maneuver is expressed with the relationships 

'Y r [x(t f )]=e r (t f )-e r =Q (r = 1 , ...,«) ( 16 ) 

where 6 r are specified constants. Similarly, a requirement that the payload and each body belonging to the 
manipulator be motionless at the final time can be stated as 

^» + r[*('/)J = ^r('/) = ° = — >«) U 7 ) 

Thus, in the problem of interest, q = 2 n. 

In what follows, the problem of bringing the payload to rest at the end of the LSMS maneuver is solved with 

C. Discontinuous Control 

The system under consideration is at rest in an inertial reference frame when co r = 0 and (O r = 0 (r = 1,..., 5). 
The condition of rest is possible when 6 X , 0 2 , and 0 3 take on any values whatsoever, and when 0 4 = 0 5 = 0 . 
Certain values of l and u 3 are of course required: the axis of the waist joint is vertical, so it is readily seen 
that rest can occur with i\ = 0. but u 2 and u 3 are in general not zero. The motors that control the shoulder joint 
and the elbow joint are designed such that they are able to prevent motion at the joint when no electric current is 
applied. For example, a motor containing a worm gear can function in this manner. Thus, motor torque can be 
produced either by application of current or by the action of a brake; the first type of torque should be included in 
the function L of Eq. (13), but the brake torque should not be. Such an approach renders ll 2 discontinuous when 
included in I; it vanishes when co 2 = 0 but takes on a nonzero value otherwise. The same holds true for u 3 . This 
complicates the formulation of the optimal control problem and makes numerical solution more difficult than it 
would be otherwise, as discussed presently. 

D. Numerical Solution of the Optimal Control Problem 

Methods for obtaining a numerical solution to an optimal control problem are categorized either as indirect, or 
direct. 

Indirect methods involve solving the well-known Euler-Lagrange equations within the framework of a two-point 
(or multi-point) boundary value problem as discussed, for example, in Ref 1 1 . Prior to treating the LSMS discussed 
in this paper, methods given in Refs. 1 1 and 12 were applied to simplified models that did not include revolute joints 
at the shoulder or elbow. First, the payload was represented by a simple pendulum suspended from a cart translating 
along the outstretched arm. and no waist joint was included in the model. Subsequent models included waist joint 
motion, and a universal joint rather than a revolute joint was used to attach the pendulum to the cart. Encouraging 
results were obtained in these cases. However, the indirect methods failed when applied to the model presented 
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here. Unfortunately, there are widely recognized shortcomings of the mdirect method, including the requirement for 
good initial guesses of the time histories of states and costates, a small convergence radius, and so on. 

On the other hand, the direct method does not involve the costates, and deals with the states and controls only. In 
this method, the states and/or controls are discretized in some maimer. The system differential equations, the 
performance index, and the constraints are approximated by some numerical methods using the discrete values of 
the states and/or controls. In the end, the optimal control problem is transcribed into a nonlinear program with the 
discrete states and/or controls being the decision variables. The nonlinear program is then solved with a standard 
optimization technique. The direct method is advantageous over the mdirect method in that it does not require a 
priori knowledge of the solution to start the process, and often it converges to an acceptable solution starting with 
only a rough guess. However, because of the discretization, the solution obtained with the direct method is not 
considered as accurate as the one obtained with the indirect method. Never-the-less, for practical purposes the 
former can be considered a good representation of the real solution when enough discrete nodes are used. The 
failure to obtain solutions for this problem with the indirect method leads us to employ the direct method. 

EZopt 1 is used to obtain the numerical solutions presented in this paper. EZopt is a software package that solves 
optimal control problems using the direct method. The entire time interval from t 0 to t f is divided into small 

segments by discrete nodes, the number and locations of which are set by the user. A collocation discretization 
scheme is used; states and controls are discretized at each node, with controls held constant between two 
neighboring nodes. (EZopt also allows differential inclusion.) The midpoint method is used to solve the differential 
equations over the interval between two neighboring nodes. The large-scale sparse optimizer SNOPT discussed in 
Ref. 1 3 is used to solve the resulting nonlinear program for the discrete state and control variables. 

As mentioned previously, AUTOLEV is used to obtain algebraic expressions for the differential equations of 
state (14). In addition, the software is employed to obtain expressions for the partial derivatives df / dx and 
df I dll needed to effect a numerical solution. AUTOLEV is instructed to write FORTRAN code containing the 
necessary expressions, and the code is used by EZOPT. 


VII. Optimal Control Results 

Numerical results are presented in this section for three cases, each of which treats the brake mechanism 
differently. The results are presented in the order of increasing fidelity in the solutions. In the first case, the brake is 
not considered in the formulation: thus, all torques are supplied by the actuators. The second case incorporates the 
brake by using a smooth approximation of the step function as a mathematical model for brake engagement and 
disengagement. This approach involves numerical difficulties and turns out to be impractical, but it provides a 
significant amount of insight into how the joints and links behave with the brakes present. Information regarding the 
timing and duration of brake engagement is particularly helpful. The third case builds upon this information, and 
uses a phased approach to obtain a more precise problem formulation. In the third approach the entire maneuver is 
divided into phases, or intermittent time intervals during which the brakes are engaged or disengaged. During a 
phase when the brakes are engaged, explicit constraints are placed on the joint angular speeds so that they remain 
zero, and the torques are not penalized in the performance index. The phases are pieced together with the conditions 
that the states are continuous at the switching times between two neighboring phases. As formulated, the phased 
approach treats the brake as intended and thus leads to a more precise solution than the second case where the 
approximate model of the brake is used. 


1 Developed by Analytical Mechanics Associates, Inc. under a NASA SBIR contract. 
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For all three cases, the following initial and final joint angles are considered. 


Table 2. Initial and fmal joint angles 


r 

(deg) 

(deg) 

1 

0 

60 

2 

20 

60 

3 

-20 

-60 

4 

0 

0 

5 

0 

0 


The initial and final joint speeds are all zero. The maneuver time t f is 60 seconds. The angular speeds (D r (t) are 
bounded by ±10 deg/s for r = 1, 2, 3, and by ±20 deg/s for r = 4, 5. The motor torques u r { f) are bounded by ±3260 
Nm for r = 1, by ±14620 Nm for r = 2, and by ±6520 Nm for r = 3. 

A. Results Without Brakes 

As can be seen by inspecting the hardware design, a tremendous amount of torque must be applied at joints 2 and 
3 to counter the moment due to gravity acting on the links and the payload. Without the brakes, the sole sources of 
torque are the electric motors at the active joints. In this approach, the integrand in Eq. (13) takes on the following 
form, 

L(x,u,t ) = ~w, 2 (t) + ]-u 2 2 (t) + ]-u 3 (t) (19) 

£ £ ** 

Therefore, an energy-saving maneuver would try to reduce the torques at these joints as much as possible. One way 
of achieving this is to reduce the moment arms to the mass centers where the gravitational forces act. This strategy 
is evident in the numerical results, with the joint angles and actuator torques shown in Fig. 9, and the angular speeds 

shown in Fig. 10. The solution is obtained with 201 equally-distanced nodes. It is seen from the time histories of 0 2 , 

0 3 , 0 4 , and 0 5 that the robot quickly drops the links outboard of the shoulder so that they hang down from the 

shoulder, maintains that configuration for as long as possible, and then heads toward the desired final configuration. 
In the hanging configuration the moments of the gravitational forces about the joints vanish, and no torque is 
required at joints 2 and 3. Also, the moment of inertia of the entire system about the vertical axis is minimized, 
which in turn minimizes the torque required to turn the waist joint. As seen in the torque plot, the torques at joints 2 
and 3 are close to zero during the middle section of the maneuver when the links are kept close to the hanging 
configuration. 

B. Results With Brakes 

Unfortunately, the hanging configuration is not likely to be a practical one. The range of motion at the shoulder 
joint will not pennit this configuration because the payload and forearm will make contact with the ground. 
Therefore, torque will always be required at the shoulder and elbow joints to counteract gravity, whether the joints 
are moving or not. As discussed previously, each motor contains a brake that applies torque to prevent motion at the 
joint in the absence of electrical power. Two techniques for treating these brakes have been considered, and 
numerical results obtained with each technique are now presented. 

B.l A solution using an approximate model of the brake 

The brake is a mechanically favorable design, but it presents a steep challenge to the optimal control problem 
formulation because with the brake the torque from the electric motor becomes a discontinuous function of the joint 

speed. Since the electric motors are engaged only when joints 2 and 3 are moved, the torques ll 2 and u 3 should not 
be penalized in the performance index when co 2 = 0 and co 3 = 0 , respectively. That is, the integrand of the 
performance index in Eq. (19) can be written as 
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(20) 


Z(a; u, t) = ^ i* 2 (i) + 1 [*© 2 ) • «* W] 2 + 1 W® 3 ) • w s(0] 2 

where z(-) is a step function. However, the new performance index is a discontinuous and non-smooth function of 
0 ) 2 and co 3 , which cannot be handled within the EZopt environment. To circumvent this difficulty, a smooth 
approxhnation of the step function is used. That is, the step functions in Eq. (20) are replaced with 
z(co 2 ) = atan(c- co 2 )I(ttI2) 

and 

z(co 3 ) = atan(c- co 3 )I(ttI2) 

where the coefficient C controls the steepness of the function z(-) rising from 0 to 1. C = 100 is used to obtain 
the numerical results shown here. 

The numerical results are illustrated in Figures 11 and 12. The solution is obtained with 201 equally-distanced 
nodes. From the time histories of the joint angles and angular speeds, it can be seen that the boundary conditions are 
satisfied. Because the brake is incorporated in the problem formulation, the robot does not go through the vertically 
hanging configuration. Instead, the time histories of 6 , and 0 3 indicate the existence of periods when joint 2 or 
joint 3 moves extremely slowly. For all practical purposes, the two joints during these periods can be considered 
motionless. However, the joint speeds C0 2 and co } exhibit chattering behavior during these time intervals. This is 
due to the numerical difficulties brought on by the steep gradients of the functions z(-) at very low joint speed. In 
addition, the solution consists of too many such intervals (some are very short) when joints 2 and 3 are held 
motionless (at least 7 for joint 2 and 15 for joint 3), which could cause the robot to stop and go too frequently to be 
considered practical. 

B.2 Precise solution with a phased approach. 

The previous solution may not be numerically and practically sound, but it provides tremendous insight into how 
the links and joints behave when the brakes are included. For example, it reveals how many times and when the 
active joints may move or be held motionless. With this knowledge, a phased approach can be taken, and the brakes 
can be modeled precisely. 

Based on the plot of 0 2 in Fig. 1 1, the motion of joint 2 can be divided into three phases. The first phase is from 
0 to 38 seconds, during which the joint is held fixed by the brake. The second phase spans from 38 to 52 seconds, 
and the electric motor is engaged to move the joint. In the last phase, from 52 to 60 seconds, the joint is again held 
still by the brake. Similar behavior can be seen in the plot of 0 3 in Fig. 11. 

In the numerical results in this section, the entire robot maneuver is divided into six phases. The time intervals 
and behaviors of 0 2 and 0 3 in each phase are summarized in Table 3. The time intervals of the phases are only an 
initial guess, and the switching times between phases are adjusted by the optimization process. During the phases 
when 0 2 or 0 3 is to be held constant, explicit constraints such as co 2 — 0 or C0 3 = 0 are enforced, and thus, the 

joint torques during these phases are not penalized in the performance index at all. The initial and final conditions 
are enforced at the beginning of phase 1 and the end of phase 6, respectively. The phases are pieced together by the 
condition that the states remain continuous across the switching time between two consecutive phases. The number 
of nodes for each phase is also shown in Table 3. Within each phase, the nodes are equally distanced. 


Table 3. Characteristics of each phase. 


Phases 

1 

2 

3 

4 

5 

6 

Guessed time interval 
(seconds) 

0-22 

22-38 

38-42 

42-44 

44 - 52 

52 - 60 

Q-, constant? 

Yes 

Yes 

No 

No 

No 

Yes 

0 3 constant? 

No 

Yes 

No 

Yes 

No 

No 

Number of nodes 

201 

601 

21 

56 

26 

101 
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The numerical results are shown in Figures 13 and 14. It can be seen that the length of each phase is changed 
from the initial guess. The boundary conditions are satisfied: i.e., all joints and the payload start and end at rest. 
Unlike the approximate solution in the previous subsection, there is no chattering behavior of the angular speeds of 
joints 2 and 3 during the intervals when the joint angles are held constant. The torque plot shows that actuator 
torques z/ 7 and « 3 are applied only when the joints 2 and 3 are in motion. Finally, this solution is also more 
practical than the approximate solution because it has much fewer stop-and-go maneuvers. 

It should be pointed out that the solution obtained with the second approach is indispensible in the sense that 
without it there is no better way to know a priori how many phases to have and when the phases switch in the phased 
approach. On the other hand, the solution from the phased approach should not be considered the best solution 
because the number of phases cannot be adjusted by the optimization process. However, the advantage of the phased 
approach is that the user controls the number of phases and the number of stop-and-go maneuvers for the active 
joints. 
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Figure 9. Joint angles and actuator torques when no brakes are used. 
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Figure 10. Angular speeds when no brakes are used. 
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Figure 11. Joint angles and actuator torques when a smoothed step function is used. 
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Figure 12. Angular speeds when a smoothed step function is used. 
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Figure 13. Join! angles and actuator torques with a 6-phase approach. 
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Figure 14. Angular speeds with a 6-phase approach. 
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VIII. Closing Comments 

A natural enhancement to the current tele-operated (person in the loop) control is automated control. Automated 
control is necessary for several reasons including the ability to direct the LSMS from remote sites (for example the 
lunar outpost or earth) and to enable interaction with global planning tools to evaluate different concepts of 
operations. The joint angle sensors necessary to provide an operator with situational awareness during tele-operated 
maneuvers are directly applicable to automated control, thus no burden is placed on the system design to support 
automated control. 

Detail of the design and installation of joint angle sensors to support automated operations and situational 
awareness for remote operator were discussed. Then the forward kinematics were developed which can be used to 
locate the wrist in inertial coordinates when the joint angles are known. Using these equations a procedure was 
provided to obtain the inverse kinematic equations using Mathematica. The inverse kinematic equations allow 
direct translation from Cartesian goals in the inertial frame to joint level commands to achieve those goals. For 
example, the inverse kinematics enable Cartesian moves of the LSMS in either tip or potentially more useful camera 
defined frames providing intuitive motion control to the operator. 

The forward and inverse kinematic equations allow a variety of schemes to be used to maneuver the LSMS to 
optimize the maneuver. One such scheme will be described in detail that eliminates undesirable swinging of the 
payload at the conclusion of a maneuver. Three approaches involving optimal control techniques are discussed, and 
numerical results obtained with each approach are provided. Of the three, the most promising approach accounts for 
a feature of a joint motor that allows it to serve as a brake in the absence of electrical power. Torque applied by the 
brake is not penalized in the performance index, but torque applied by the electric motor is penalized. The entire 
maneuver is divided into several phases during which, individually, the brakes at the shoulder and elbow joints 
either are, or are not, engaged. Using this approach, joint trajectories can be identified that eliminate the undesirable 
swing of the payload at the conclusion of the maneuver, enabling precision control of the payload using the LSMS. 
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Appendix A: Denavit Hartenburg Notation for LSMS 



Figure 15. Local Reference Frames for Denavit-Hartenburg Parameter Definition. 


The purpose of this appendix is to describe a set of the Denavit-Hartenburg parameters for the LSMS. The 
description of these parameters will follow the notation given by Craig. 8 Denavit-Hartenburg parameters are a 
reduced set of parameters used to describe the relationship between links in a robotic manipulator. Four parameters 
are used to describe the relative location of two frames. They are: 

a a /v 

link twist = the angle between and ^j+i measured about ^ j , 

A A A 

link length = the distance from to ^j+\ measured along X - ? 

A A A 

joint angle = the angle between ^ i-\ to measured about , and 

A A A 

„ , _ rl , „ Y Y 7 

link offset u 7 = the distance from i-\ to / measured along . 

A A 

where X ■ and are unit vectors in the i ,h coordinate frame. Using these definitions the homogenous transform 
describing frame i in frame i-1 is given by 

cos(d?) -sin(# ; ) 0 a j _ l 

sin(# ; ) cos(a ( _, ) cos(^)cos(«,_ 1 ) — sin(^_j ) -sin(a i _ 1 )d j 
sin(<9 f ) sin(^_ 1 ) cos(^ ; )sin(Q' ( _ 1 ) cos^,,) cos(a j _ l )d 

0 0 0 1 

Referring to Fig. 15 the DH parameters are: 


\T = 
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Table 4, Denavit-Hartenburg parameters for LSMS. 


i 

a i- 1 

a i - 1 

9, 

< 

1 (waist) 

0 

0 

0, 

0 

2 

0 

0 

0 

n R 

3 

71 

0 


0 

4 

0 

Rp 

0 , 

0 

5 (wrist) 

0 

* 0 -p) 

0 

0 
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Appendix B: Mathematica Code to Generate Inverse Kinematic Solutions 


Clear All [Evaluate [Context [] <> " * "]] 


Cos[01] — Sin[01] 0 0 


Tground2waist[61_]: = ( 


Sin mil r#i«rnn n n 



0 0 0 1 
Cos[02] 0 Sin [02] 0 


Twaist2 shoulder f 02_] : = 



0 0 0 1 


Cos [03] 0 Sin[03] QR 


Tshoulder2elbow[03_]: = ( 


n in n 



0 0 0 1 
10 0 R(l-(3 ) 

Tdbow2wrist[04J:= (° J J ° Q ) 

0 0 0 1 

Tground2wrist[01_, 02_, 03 _, 04J: 

= Tground 2-waist [01]. Twaist2shoulder [02].Tshoulder2elbow[03].Telbow2wrist[04] 


Vwristsimp = MatrixForm [Full Simplify [Vwrist]] 

Vwristsimp = Vwristsimp[[l]] 

soln = SoLve[{Vwristsimp [[!]][[!]] == tx, Vwristsimp [[2]] [[1]] == tv, Vwristsimp [[3] ][[!]] = 


0 


Vwrist = Tground 2wrist[01, 02,03, 04]. (^) 

1 


= tz}. {01, 02, 03}] 
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Appendix C: Mass Properties and Geometry 


The mass of each body is recorded in Table 5. 


Table 5. Mass 


Body 

Mass 

(kg) 

Kin» Post 

88.8735 

Arm 

81.7850 

Forearm 

32.7304 

Lifting Link 

3.5974 

Payload 

150.0 


Moments of inertia and products of inertia, referred to collectively as inertia scalars, are with respect to the mass 
center of each body, and are reported in Table 6 for the reference configuration illustrated in Fig. 7. 


Table 6. Inertia Scalars 


Body 

In 

(kg-m 2 ) 

I22 

(kg-m 2 ) 

I33 

(kg-m 2 ) 

1.2 

(kg-m 2 ) 

I23 

(kg-m 2 ) 

1 1 3 

(kg-m 2 ) 

King Post 
Arm 

198.7825 

230.1985 

32.7615 

-1.2030 

-2.2441 

-33.2139 

33.3894 

165.9743 

133.8360 

-0.9035 

1.5218 

24.8087 

Foreann 

4.8377 

60.1935 

55.5114 

-0.0067 

-0.0010 

9.2589 

Lifting Link 
Payload 

0.5606 

0.5541 

0.0076 

0 

0.0025 

0.0052 

33.6794 

27.0725 

13.7197 

0 

0 

0 


For each body, the position vector from a point on the axis of the mboard joint to the mass center is reported in 
Table 7, followed by the position vector from the mass center to a point on the axis of the outboard joint. Numerical 
values are given for the reference configuration illustrated in Fig. 7. In the case of the king post, the mboard and 
outboard joints are, respectively, the waist joint and the shoulder joint. The inboard and outboard joints for the arm 
are, respectively, the shoulder joint and the elbow joint. The designation of inboard and outboard joints for the 
remaining bodies follows similarly. The point on the axis of the waist joint is fixed in both an inertial reference 
frame and the king post. The point on the axis of the shoulder joint is fixed in both the king post and the arm. Other 
points on each joint axis are likewise fixed in each of two adjacent bodies. 


Table 7. Locations of mass centers and joints 


Body 

Inboard joint to mas 
r i r 2 

(s) (m) 

s center 

r 3 

( m J 

Mass center to outboard joint 
ri r 2 r 3 

(m) 1 1m) 1m) 

King Post 

gir.ynMI 


1.1254 

■m 

0.0200 

2.5230 I 

Arm 




3.2145 | 



Foreann 



0.4422 

mmm\ 



Lifting Link 



-0.2730 

0 


mmuM 

Payload 

0 

0 

-0.6858 

1 1 1 
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